#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <limits.h>
#include "validation.h"
#include "profiler.h"

/* No inlining helps debugging... */
#define INLINE inline
//#define INLINE

/* Feed data from a big flash array, or not... */
#define STATIC_DATA 0
/* Data fed from the serial port */
#define SERIAL_DATA 0
/* If neither is set, data is fed from the ADC */

/* Multiply input data by this factor */
#define DATA_MULTIPLIER 8

#define H_BITS 5
#define H_SIZE  (1 << H_BITS)
#define H_MASK ((1 << H_BITS)-1)

#define EPS_WINDOW_BITS 3
#define EPS_WINDOW_SIZE (1 << EPS_WINDOW_BITS) /* We average over 8 buffers to compute the EPS. */
#define EPS_WINDOW_MASK ((1 << EPS_WINDOW_BITS) - 1)

static void fill_until(offset_t until);
#if !ON_PC
#include <io.h>
#include <signal.h>

/* Stuff for RS232 communication */
#define ASK '?'
#define ACK 'A'
#define NACK 'N'
#define RESET 'R'
#define START '*' /* Start of a message */
#define END '%' /* End of the message */
#define DONE 'D'

#if SERIAL_DATA
#define RS_BLOCK 512

struct datablk {
	int16_t length;
	int16_t csum;
	int16_t buffer[RS_BLOCK];
} __attribute__((__packed__));

static struct datablk data;
/* Current position in the datablk */
static int data_index = 0;
static void readuart();
#endif
#endif

/* ------------------ Delineation variables ------------------- */
static data_t in_data[BUFFER_SIZE];
static data_t h_data[4][H_SIZE];	/* Note: Starting from 0, i.e. h_1 is at h_data[0] */
static data_t q_data[5][BUFFER_SIZE];	/* Note: Starting from 0 */
static data_t eps_data[4];		/* Starting from 0 */
static data_t eps_data_int[4][EPS_WINDOW_SIZE];

static offset_t offset = 0; /* Last valid data real offset (i.e. non-circular) */
offset_t last_offset = 0x7FFFFFFFL; /* Last non-circular offset (EOF) */
  
static offset_t last_r = 0; /* Last detected R peak. */
static offset_t this_r = 0; /* Currently detected R peak. */
static offset_t rr = 150; /* Averaged detected RR interval, starting with 100bpm as default value */
static int n_valid = 0;

static offset_t this_qrs_end; /* Last end of QRS complex (for blanking) */
static offset_t this_t_end; /* Last end of T complex (for blanking as well) */

static offset_t pr1 = 0; /* Averaged interval between P (1st modmax) and R, to improve P detection. */
static offset_t pr2 = 0; /* Averaged interval between P (2nd modmax) and R, to improve P detection. */

static data_t epsilon_t, epsilon_p;
static data_t epsilon_t_5, epsilon_p_5;

/* Current correct (=non-isolated, non-redundant) maximum and minimum (4) [Li]) */
/* NB: I don't really really know how to behave if more than 3 points are found
 * within 120ms anyway, so I hardcode the value to 2, and try to do something clever */
static offset_t ok_maxmin[2][5]; /* [.][0] is unused */
static int ok_maxmin_valid = 0; /* Number of valid ok_maxmin items */

static offset_t searchback_r = 0;
/* ------------------------------------------------------------ */

#if ON_PC
static FILE* datafile; /* Data file descriptor */

/* Reset the above to their original values */
static void reset_variables() {
	memset(in_data, 0, sizeof(in_data));
	memset(h_data, 0, sizeof(h_data));
	memset(q_data, 0, sizeof(q_data));
	memset(eps_data, 0, sizeof(eps_data));
	memset(eps_data_int, 0, sizeof(eps_data_int));

	offset = 0;
	last_offset = 0x7FFFFFFFL;
	
	last_r = 0;
	this_r = 0;
	rr = 150;
	n_valid = 0;

	this_qrs_end = 0;
	this_t_end = 0;

	pr1 = pr2 = 0;

	epsilon_t = epsilon_p = 0;
	epsilon_t_5 = epsilon_p_5 = 0;
	ok_maxmin_valid = 0;

	searchback_r = 0;
}
#endif

static INLINE void prefetch(offset_t i) {
	if (i+2 >= offset && i < last_offset) {
		fill_until(i+2);
	}
}

static INLINE unsigned int convh(offset_t i) {
	return i & H_MASK;
}

static INLINE unsigned int conv(offset_t i) {
//#define SANITY_CHECK 1
#if SANITY_CHECKS
	if (i+BUFFER_SIZE < offset) {
		dbg_printf("SANITY CHECK: Tried to access data below data start i="POT"+%u/"POT".\n"
			   "@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@\n", i, BUFFER_SIZE, offset);
	}
	if (i > last_offset) {
		dbg_printf("SANITY CHECK: Tried to access data after last_offset data start "POT"/"POT".\n"
			   "@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@\n", i, last_offset);
	}
	if (i >= offset) {
		dbg_printf("SANITY CHECK: Prefetching failed...\n@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@\n");
		fill_until(i+1);
	}
#endif
	return i & BUFFER_MASK;
}

static INLINE unsigned int conv_nofetch(offset_t i) {
	return i & BUFFER_MASK;
}

#define IN(i) in_data[conv_nofetch(i)]
#define H(scale, i) h_data[scale-1][convh(i)]
#define Q(scale, i) q_data[scale-1][conv(i)]
#define Qn(scale, i) q_data[scale-1][conv_nofetch(i)]
#define EPS(scale) eps_data[scale-1]
#define EPS_INT(scale, window) eps_data_int[scale-1][window]

#if (defined(__i386__)||defined(__amd64__))
#define USE_ASM 0
#else
#define USE_ASM 1
#endif

static INLINE data_t highpass(data_t d1, data_t d2) {
#if !USE_ASM
	int32_t big = 2*((int32_t)d1-(int32_t)d2);
	if (big >  0x7fffL) return 0x7fffL;
	if (big < -0x8000L) return -0x8000L;
	return (data_t)big;
#else
	asm("mov %[d1],r13\n\t"       // r13 = d1
	    "sub %[d2],%[d1] \n\t"    // ret=ret-d2
	    "bit #0x100,r2 \n\t"      // test overflow bit
	    "jnz +8\n\t"             // jump to ov if overflow
	    "rla %[d1]\n\t"           // ret = ret*2 (rotate left)
	    "bit #0x100,r2 \n\t"      // test overflow bit
	    "jz +8\n\t"               // jump out if no overflow
//overflow:
	    "rlc r13\n\t"             // Get the MSB of r13 (=d1) in the carry
	    "clr %[d1]\n\t"           // Clear ret
	    "addc #0x7FFF, %[d1]\n\t" // ret=0x7FFF+(MSB[d1])
	    : [d1] "=r" (d1)
	    : "[d1]" (d1), [d2] "r" (d2) : "r13", "r2", "cc");
	return d1;
#endif
}

static INLINE data_t lowpass(data_t d1, data_t d2, data_t d3, data_t d4) {
#if !USE_ASM
	int32_t big = (int32_t)d1 + 3*(int32_t)d2 + 3*(int32_t)d3 + (int32_t)d4;
        /* Rounding to the closest integer, but if in the middle, round down
	 * It means, if we get 0.5, we round to 0, if 0.51, we round to 1.
         * This is not the common way of doing it, but this gives best results  */
	if (big >= 0)
		big = (big+3)>>3;
	else
		big = (big+4)>>3;
	if (big >  0x7fffL) return 0x7fffL;
	if (big < -0x8000L) return -0x8000L;
	return (data_t)big;
#else
	asm("clr r10\n\t" // Count the number of negative numbers, and put -n in r10
	    "bit #0x8000, %[d2]\n\t"
	    "adc r10\n\t"
	    "bit #0x8000, %[d3]\n\t"
	    "adc r10\n\t"
	    "rla r10\n\t"
	    "bit #0x8000, %[d1]\n\t"
	    "adc r10\n\t"
	    "bit #0x8000, %[d4]\n\t"
	    "adc r10\n\t"
	    "inv r10\n\t"
	    "inc r10\n\t"
	    "add %[d2], %[d1]\n\t" // Add the numbers
	    "adc r10\n\t"         
	    "rla %[d2]\n\t"       
	    "add %[d2], %[d1]\n\t"
	    "adc r10\n\t"         
	    "add %[d3], %[d1]\n\t"
	    "adc r10\n\t"         
	    "rla %[d3]\n\t"       
	    "add %[d3], %[d1]\n\t"
	    "adc r10\n\t"         
	    "add %[d4], %[d1]\n\t"
	    "adc r10\n\t"
	    "bit #0x0004, r10\n\t" // Sign bit
	    "jc +8\n\t"
	    "add #0x0003, %[d1]\n\t" // Positive, do +3
	    "adc r10\n\t"
	    "jmp +4\n\t"
	    "add #0x0004, %[d1]\n\t" // Negative, do +5
	    "adc r10\n\t"
	    "rrc r10\n\t" // Shift left
	    "rrc %[d1]\n\t"
	    "rrc r10\n\t"
	    "rrc %[d1]\n\t"
	    "rrc r10\n\t"
	    "rrc %[d1]\n\t"
	    : [d1] "=r" (d1)
	    : "[d1]" (d1), [d2] "r" (d2), [d3] "r" (d3), [d4] "r" (d4) : "r10", "r2", "cc" );
	return d1;
#endif
}

/* For me delay[1] should be 1... but 0 seemed to give better results...
 * Actually both are ok */
/* This one starts from 1 */
static const offset_t delay[] = { 0, 0, 2, 6, 14, 30 };

static INLINE void filter(offset_t i) {
//	PROF_START;
	Qn(1,i) = highpass(IN(i), IN(i-1)); /* Delay: 0.5 */
	H(1,i)  = lowpass(IN(i), IN(i-1), IN(i-2), IN(i-3)); /* Delay: 1.5 */
	Qn(2,i) = highpass(H(1, i), H(1, i-2)); /* Delay: 1.5+1=2.5 */
	H(2,i)  = lowpass(H(1, i), H(1, i-2), H(1, i-4), H(1, i-6)); /* Delay: 1.5+3=4.5 */
	Qn(3,i) = highpass(H(2, i), H(2, i-4)); /* Delay: 4.5+2=6.5 */
	H(3,i)  = lowpass(H(2, i), H(2, i-4), H(2, i-8), H(2, i-12)); /* Delay: 4.5+6=10.5 */
	Qn(4,i) = highpass(H(3, i), H(3, i-8)); /* Delay: 10.5+4=14.5 */
	H(4,i)  = lowpass(H(3, i), H(3, i-8), H(3, i-16), H(3, i-24)); /* Delay: 10.5+12 = 22.5 */
	Qn(5,i) = highpass(H(4, i), H(4, i-16)); /* Delay: 22.5+8=30.5 */
//	PROF_STOP;
}

static int32_t isqrt32(int32_t input) {
	if (input < 0) return -1;
	uint16_t x = 0;
	int32_t rmdr = 0;
	int count = 16;
	
	//printf("count=%u, rmdr=%lu, input=%lu (F)\r\n", count, rmdr, input);

	while (count > 0 && rmdr == 0) {
#if !USE_ASM
		rmdr = (input >> 30) & 0x03;
		input = input << 2;
#else
		asm("clrc\n\t"
		    "rlc %A[input]\n\t"
		    "rlc %B[input]\n\t"
		    "rlc %A[rmdr]\n\t"
		    "rlc %A[input]\n\t"
		    "rlc %B[input]\n\t"
		    "rlc %A[rmdr]\n\t"
		    : [input] "=r"(input), [rmdr] "=r"(rmdr)
		    : "[input]"(input), "[rmdr]" (rmdr) : "cc");
#endif
		count--;
	}

	if (count > 0) {
		x++;
		rmdr--;
	}

	while (count > 0) {
#if !USE_ASM
		rmdr = (rmdr << 2) | (((input & 0xC0000000) >> 30) & 0x03);
		input = input << 2;
#else
		asm("clrc\n\t"
		    "rlc %A[input]\n\t"
		    "rlc %B[input]\n\t"
		    "rlc %A[rmdr]\n\t"
		    "rlc %B[rmdr]\n\t"
		    "rlc %A[input]\n\t" /* carry is 0 here, for sure */
		    "rlc %B[input]\n\t"
		    "rlc %A[rmdr]\n\t"
		    "rlc %B[rmdr]\n\t"
		    : [input] "=r"(input), [rmdr] "=r"(rmdr)
		    : "[input]"(input), "[rmdr]" (rmdr) : "cc");
#endif
		x = x << 1;
		int32_t diff = rmdr - ((x<<1)+1);
		if (diff >= 0) {
			x++;
			rmdr = diff;
		}
		count--;
	}

	return x;
}

#if 0
static int32_t isqrt32_newton(int32_t input) {
	if (input < 0) return -1;
	int32_t xk = 0, xk_1;
	/* Try to guess a correct value to start with,
	 * since each iteration is very expensive (division is expensive) */
	if (input & 0xF0000000L)
		xk = 0xB505;
	if (input & 0x0F000000L)
		xk += 0x2D41;
	if (input & 0x00F00000L)
		xk += 0x0B50;
	if (input & 0x000F0000L)
		xk += 0x02D4;
	if (input & 0x0000F000L)
		xk += 0x00B5;
	if (input & 0x00000F00L)
		xk += 0x002D;
	if (input & 0x000000F0L)
		xk += 0x000B;
	if (input & 0x0000000FL)
		xk += 0x0003;

	while (1) {
		xk_1 = (xk + input/xk + 1)/2;
		if (xk_1 == xk) break;
		xk = xk_1;
	}

	return xk_1;
}
#endif

/*
static int16_t isqrt16(int16_t input) {
	if (input < 0) return -1;
	int16_t xk, xk_1;
	xk = xk_1 = 1;
	int i = 0;
	
	while (1) {
		xk_1 = (xk + input/xk + 1)/2;
		if (xk_1 == xk) break;
		xk = xk_1;
		i++;
		if (i > 1000)
			dbg_printf("sqrt stuck (input=%d)!\n", input);
	}

	return xk_1;
}*/

static data_t rms(int scale, offset_t offset, offset_t asize) {
	int32_t val = 0;
	data_t out;
	offset_t i;

	/* TODO: Check overflows! */
	for (i = offset; i < asize; i++) {
		val += (int32_t)Q(scale, i)*(int32_t)Q(scale, i);
	}
	dbg_printf("First data: %d, sum="POT", b="POT"\n", Q(scale, offset), val, val/(asize-offset));

	out = (data_t)isqrt32(val/(asize-offset));
	return out;
}

#if ON_PC /* Data by reading file */
static int multiplier = DATA_MULTIPLIER;
static int fetchdata() {
	char buffer[128];

	if (!fgets(buffer, 128, datafile)) {
		return 0;
	}

	IN(offset) = (data_t)atoi(buffer)*multiplier;
	return 1;
}
#elif STATIC_DATA /* Data on a big array in flash */
#include "tmp/data.h"
static int data_input_offset = 0;
static INLINE int fetchdata() {
	if (data_input_offset < (sizeof(input_data)/sizeof(int16_t))-1) {
		IN(offset) = input_data[data_input_offset]*DATA_MULTIPLIER;
		data_input_offset++;
		return 1;
	}
	return 0;
}
#elif SERIAL_DATA /* Data fed from the serial port */
static INLINE int fetchdata() {
	/* Nothing anymore */
	if (data.length == 2)
		return 0;

	if (data_index == data.length-2) {
		readuart();
		data_index = 0;
	}

	IN(offset) = data.buffer[data_index]*DATA_MULTIPLIER;
	data_index++;
	return 1;
}
#else /* Data fed from the ADC */
void wait(long long c);

static offset_t adc_offset = 0;
static INLINE int fetchdata() {
	while (1) {
		/* Prevent race conditions on adc_offset, disable interrupts */
		dint();
/*		if (!(offset & 0xff))
		printf("adc="POT"//off="POT"\r\n", adc_offset, offset);*/
		if (adc_offset > offset) {
			eint();
			return 1;
		}
		eint();
		wait(10000L);
	}
}
#endif

static void fill_until(offset_t until) {
	PROF_START;
	offset_t i;

	//dbg_printf("fill_until ==\n");

	while (offset < until) {
		//dbg_printf("fill_until: => %d/%d\n", offset, until);

		if (!fetchdata()) {
			dbg_printf("fill_until end reached ("POT")\n", offset);

			if (last_offset != offset) {
				last_offset = offset;

				/* Printing */
				data_printf("in = [ in ");
				for (i = last_offset-conv_nofetch(last_offset); i < last_offset; i++)
					data_printf("%d ", IN(i));
				data_printf("];\n");
				
				int j;
				
				for (j = 1; j < 6; j++) {
					data_printf("q%d = [ q%d ", j, j);
					for (i = last_offset-conv_nofetch(last_offset); i < last_offset; i++)
						data_printf("%d ", Q(j,i));
					data_printf(" ];\n");
				}
			}

			break;
		}

		filter(offset);
		offset++;

//		dbg_printf("fill_until: >> %d/%d\n", offset, until);

		if (conv_nofetch(offset) == 0) {
			EPS(4) = EPS(4)*2;
			/* epsilon 1-4 */
			for (i = 1; i <= 4; i++) {
#if PRECISE_EPSILON
				data_t neweps;
				if (i == 4) EPS(i) = EPS(i)*2;
				dbg_printf("epsilon EPS ("POT", "POT")\n",
					   offset, offset == BUFFER_SIZE ? delay[i]: offset-BUFFER_SIZE);
				neweps = rms(i, offset == BUFFER_SIZE ? delay[i]: offset-BUFFER_SIZE, offset);
				dbg_printf("computed RMS: %d\n", neweps);

				if (offset == BUFFER_SIZE) { /* First data, fill the window with the same value */
					EPS(i) = neweps;
				}
				else {
					EPS(i) = isqrt32(((int64_t)EPS(i)*(int64_t)EPS(i)*(offset-BUFFER_SIZE) + (int64_t)neweps*(int64_t)neweps*BUFFER_SIZE)/offset);
				}
				
#else
				data_t neweps;
				dbg_printf("epsilon EPS ("POT", "POT")\n",
					   offset, offset == BUFFER_SIZE ? delay[i]: offset-BUFFER_SIZE);
				neweps = rms(i, offset == BUFFER_SIZE ? delay[i]: offset-BUFFER_SIZE, offset);
				dbg_printf("computed RMS: %d\n", neweps);
				if (offset == BUFFER_SIZE) { /* First data, fill the window with the same value */
					int j;
					for (j = 0; j < EPS_WINDOW_SIZE; j++)
						EPS_INT(i, j) = neweps >> EPS_WINDOW_BITS;

					EPS(i) = neweps;
				}
				else {
					neweps = neweps >> EPS_WINDOW_BITS;
					/* Choose which window position to erase */
					int pos = (offset >> BUFFER_BITS) & EPS_WINDOW_MASK;
					EPS(i) = EPS(i)-EPS_INT(i, pos)+neweps;
					EPS_INT(i, pos) = neweps;
				}
#endif
				dbg_printf("chosen EPS: %d\n", EPS(i));
/* Moving average EPS
				if (offset == BUFFER_SIZE)
					EPS(i) = 3*neweps/4;
				else if (offset == 2*BUFFER_SIZE)
					EPS(i) = (EPS(i)+(3*neweps/4))/2;
				else
					EPS(i) = (2*EPS(i)+neweps)/3;
*/
			}
			EPS(4) = EPS(4)/2;

#if ON_PC
			for (i = 1; i <= 4; i++)
				data_printf("eps%d = %d;\n", i, EPS(i));

			/* Printing */
			data_printf("in = [ in ");
			for (i = offset-BUFFER_SIZE; i < offset; i++)
				data_printf("%d ", IN(i));
			data_printf("];\n");

			int j;

			for (j = 1; j < 6; j++) {
				data_printf("q%d = [ q%d ", j, j);
				for (i = (offset == BUFFER_SIZE) ? delay[j]: offset-BUFFER_SIZE; i < offset; i++)
					data_printf("%d ", Q(j,i));
				data_printf(" ];\n");
			}
#endif
		}
	}
	PROF_STOP;
//	dbg_printf("fill_until: <= %d/%d\n", offset, until);
}

/* First time fill-in. */
static void fill_in() {
	int i;

	data_printf("in = [];");
	for (i = 0; i < 4; i++) {
		data_printf("q%d = [];", i+1);
//		memset(&q_data[i], 0, BUFFER_SIZE*sizeof(data_t));
//		memset(&h_data[i], 0, BUFFER_SIZE*sizeof(data_t));
	}
	data_printf("q5 = [];");
//	memset(&q_data[4], 0, BUFFER_SIZE*sizeof(data_t));

	fill_until(BUFFER_SIZE);
}

/* Returns
 *  0 - if no modmax was found
 *  1 - if it was found, value is put in out.  */
static int findmodmax(int scale, offset_t offset, offset_t asize, data_t threshold, offset_t* out) {
	prefetch(offset+2);
	if (offset+2 >= last_offset || offset > asize) return 0;

	PROF_START;
	/* Current trend, 1 = increasing, 0 = decreasing */
	int trend = (Q(scale, offset+1) > Q(scale, offset));
	offset_t i;
	data_t qi;
	data_t qi_1;

	//dbg_printf("findmodmax off=%d asize=%d thres=%lf\n", offset, asize, threshold);

	qi = Q(scale, offset+1);
	for (i = offset+2; i < asize && i < last_offset; i++) {
		prefetch(i);
		qi_1 = qi;
		qi = Q(scale, i);
		//dbg_printf("findmodmax i="POT"/"POT": %d %d -- %d\n", i, last_offset, Q(scale, i-1), Q(scale, i), trend);
		if (qi > qi_1) {
			if (trend == 0 && -qi_1 >= threshold) {
				*out = i-1;
				PROF_STOP;
				return 1;
			}
			trend = 1;
		}
		else {
			if (trend != 0 && qi_1 >= threshold) {
				*out = i-1;
				PROF_STOP;
				return 1;
			}
			trend = 0;
		}
	}

	PROF_STOP;
	return 0; /* No mod max found in the range */
}

static INLINE data_t data_abs(data_t x) {
	return x > 0 ? x : -x;
}

/* max = 1, find max, else find min */
static offset_t get_minmax(int scale, offset_t from, offset_t to, int max) {
	offset_t i;
	offset_t max_q = from;

	for (i = from+1; i <= to && i < last_offset; i++) {
		prefetch(i);
		/*dbg_printf("get_max: %d %lf %lf\n",
					i, Q(scale)[max_q], Q(scale,i));*/
		if (max) {
			if (data_abs(Q(scale, i)) > data_abs(Q(scale, max_q)))
				max_q = i;
		} else {
			if (data_abs(Q(scale, i)) < data_abs(Q(scale, max_q)))
				max_q = i;
		}
	}

	return max_q;
}

/* Returns one-two, but checks we do not go below 0. */
static INLINE offset_t off_sub(offset_t one, offset_t two) {
	return one > two ? one-two : 0;
}

static INLINE offset_t off_diff(offset_t one, offset_t two) {
	if (one > two) return one-two;
	else return two-one;
}

static INLINE int data_sign(data_t x) {
	return x > 0;
}

static INLINE offset_t off_min(offset_t x, offset_t y) {
	return x > y ? y : x;
}

static INLINE offset_t off_max(offset_t x, offset_t y) {
	return x > y ? x : y;
}

static INLINE data_t data_max(data_t x, data_t y) {
	return x > y ? x : y;
}

/* Return: 0 if not found, 1 if found, value in out. */
static int detect_zero_crossing(int scale, offset_t from, offset_t to, offset_t* out) {
	offset_t i;
	/* If from is bigger, start from there, otherwise, start from to. */
	if (data_abs(Q(scale, from)) > data_abs(Q(scale, to))) {
		for (i = from; i < to; i++) {
			if (data_sign(Q(scale,i)) != data_sign(Q(scale, i+1))) {
				dbg_printf("0-crossing("POT"): %d %d\n",
							i-delay[scale], Q(scale,i),
							Q(scale, i+1));
				if (data_abs(Q(scale,i)) < data_abs(Q(scale, i+1)))
					*out = i-delay[scale];
				else
					*out = i+1-delay[scale];
				return 1;
			}
		}
	} else {
		for (i = to-1; i >= from; i--) {
			if (data_sign(Q(scale,i)) != data_sign(Q(scale, i+1))) {
				dbg_printf("0-crossing("POT"): %d %d\n",
							i-delay[scale], Q(scale,i),
							Q(scale, i+1));
				if (data_abs(Q(scale,i)) < data_abs(Q(scale, i+1)))
					*out = i-delay[scale];
				else
					*out = i+1-delay[scale];
				return 1;
			}
		}
	}
	return 0;
}

/* onset */
static offset_t detect_onset(int scale, offset_t from, data_t xi_on, int min_ok) {
	offset_t i;

	for (i = from; i > 1; i--) {
		dbg_printf("detect_onset i="POT" val=%d (%d), thres=%d\n", i-delay[scale], Q(scale,i), Q(scale, i-1), xi_on);
		if (data_abs(Q(scale,i)) < xi_on) {
			return i-delay[scale];
		}
		/* Crossing zero necessarly goes below the threshold */
		if (data_sign(Q(scale,i)) != data_sign(Q(scale, i-1))) {
			dbg_printf("detect_onset cross-zero\n");
			return i-delay[scale];
		}
		/* We only consider local minimum if all values are of the same sign */
		else if (min_ok && data_sign(Q(scale,i)) == data_sign(Q(scale, i+1)) &&
			data_abs(Q(scale,i)) <= data_abs(Q(scale, i-1)) && data_abs(Q(scale,i)) <= data_abs(Q(scale, i+1))) {
			dbg_printf("detect_onset modmin %d %d %d\n", Q(scale, i-1), Q(scale, i), Q(scale, i+1));
			return i-delay[scale];
		}
	}
	return 1;
}

/* end */
static offset_t detect_end(int scale, offset_t from, data_t xi_end, int min_ok) {
	offset_t i;
	for (i = from; i < last_offset-1; i++) {
		prefetch(i+1);
		dbg_printf("detect_end i="POT" val=%d, thres=%d\n", i-delay[scale], Q(scale,i), xi_end);
		if (data_abs(Q(scale,i)) < xi_end) {
			dbg_printf("detect_end threshold\n");
			return i-delay[scale];
		}
		/* Crossing zero necessarly goes below the threshold */
		if (data_sign(Q(scale,i)) != data_sign(Q(scale, i+1))) {
			dbg_printf("detect_end cross-zero\n");
			return i-delay[scale];
		}
		if (min_ok && data_abs(Q(scale,i)) <= data_abs(Q(scale, i-1)) && data_abs(Q(scale,i)) <= data_abs(Q(scale, i+1))) {
			dbg_printf("detect_end modmin\n");
			return i-delay[scale];
		}
	}
	return last_offset-1;
}

#define RR_MOVING_AVERAGE 4

static INLINE void add_r(offset_t r) {
	last_r = this_r;
	this_r = r;

	offset_t newr = this_r-last_r;
	if (newr > rr*2) newr = rr*2; /* Fixup for sel232 */
	if (newr < rr/2) newr = rr/2;

	dbg_printf("add_r "POT"-"POT" (%d), rr="POT"\n", last_r, this_r, n_valid, rr);
	if (n_valid > 0)
		rr = ((n_valid-1)*rr + newr)/n_valid;
	
	if (n_valid < RR_MOVING_AVERAGE)
		n_valid++;

	if (rr < 50) /* Max HBR=300 bpm = 5 beats per second */
		rr = 50;

	if (rr > 750) /* Min HBR=20 bpm = 1 beat every 3 seconds */
		rr = 750;

	dbg_printf("add_r rr now: "POT"\n", rr);

	this_qrs_end = r;
}

#define EPSILON_P 3/25 /* [Martinez] says 0.02, we take 0.12 */
#define EPSILON_T 1/4 /* [Martinez] says 0.25... */

static INLINE void compute_epsilon_pt() {
	/* Using the existing EPS(4) doesn't make much difference. (Normally we should recompute the RMS) */
/*	epsilon_t = EPS(4)*2*EPSILON_T;
	epsilon_p = EPS(4)*2*EPSILON_P;*/
	epsilon_p = 0;
	epsilon_t = 0;
	epsilon_p_5 = 0;
	epsilon_t_5 = 0;
}

static INLINE void compute_epsilon_pt_5() {
/*	if (epsilon_t_5 == 0) { // Compute when needed
		offset_t from = off_max(last_r, off_sub(offset, BUFFER_SIZE));
		data_t rms_rr = rms(5, from, this_r);
		epsilon_t_5 = rms_rr*EPSILON_T;
		epsilon_p_5 = rms_rr*EPSILON_P;
	}*/
	epsilon_p_5 = 0;
	epsilon_t_5 = 0;
}

#define P_T_GAMMA 1/8 //0.125
#define P_XI_ON 1/2 // 0.5
#define P_XI_END 9/10 //0.9
#define T_XI_ON 1/4 //0.25
#define T_XI_END 2/5 //0.4

/* @init-@end: Research zone for the main peak
 * @limit: for p wave: qrs_onset, for t wave: qrs_offset (we don't want our waves to overlap)
 * @scale: which scale to search at
 * @gamma/xi: see papers
 * @out: p or t? */
static int pt_detect(offset_t init, offset_t end, offset_t limit,
	      int scale, data_t epsilon, char out) {
//	trace_fun();
	/* Maximal modmax */
	data_t max = 0;
	offset_t max_pos = 0;
	/* Threshold to consider a modmax relevant (gamma) */
	data_t thres;
	int i;
	/* First and last relevant peak */
	offset_t nfirst = end, nlast = init;

	offset_t orig_init = init;

#define Q4_MAXMOD_NR_MAX 16
	offset_t q4_maxmod[Q4_MAXMOD_NR_MAX];
	int q4_maxmod_nr = 0;
	int found = 0;
	int tmpfound;
	int32_t maxdiff = 0; /* Maximum difference/distance (i.e. most significant wave) */
	offset_t maxdiff_p = 0; /* Maximum difference/distance position */

	while (1) {
		dbg_printf("init="POT" end="POT"\n", init-delay[scale], end-delay[scale]);
		if(!findmodmax(scale, init, end, epsilon, &init))
			break;

		q4_maxmod[q4_maxmod_nr++] = init;
		if (q4_maxmod_nr == Q4_MAXMOD_NR_MAX) {
			dbg_printf("%c_detect: BUG (q4_maxmod overflow)\n", out);
			q4_maxmod_nr--;
		}
		if (data_abs(Q(scale, init)) > max) {
			max = data_abs(Q(scale, init));
			max_pos = init;
		}

		dbg_printf("%c_detect: modmax@"POT" (%d) (%d, eps=%d)\n", out, init-delay[scale], q4_maxmod_nr-1, Q(scale, init), epsilon);
	}

	if (q4_maxmod_nr == 0) {
		dbg_printf("%c_detect: significant wave not found (scale=%d)\n", out, scale);
		return 0;
	}
#undef Q4_MAXMOD_NR_MAX

	thres = max*P_T_GAMMA;
	for (i = 0; i < q4_maxmod_nr-1; i++) {
		int next = i+1;
		dbg_printf("%c_detect: trying %d-%d (%d,%d ?< %d)\n", out, i, next,
					Q(scale, q4_maxmod[i]), Q(scale, q4_maxmod[next]), thres);

		if (data_abs(Q(scale, q4_maxmod[i])) < thres)
			continue;

		while (data_abs(Q(scale, q4_maxmod[next])) < thres) {
			next++;
			if (next >= q4_maxmod_nr)
				goto wave_out;
			dbg_printf("%c_detect: trying %d-%d (%d,%d ?< %d)\n", out, i, next,
					Q(scale, q4_maxmod[i]), Q(scale, q4_maxmod[next]), thres);
		}

		/* Sign need not to be opposite */
		/*if (data_sign(Q(scale)[q4_maxmod[i]]) == data_sign(Q(scale)[q4_maxmod[next]]))
			continue;*/

		dbg_printf("nfirst/last="POT"/"POT", now="POT"/"POT"\n", nfirst-delay[scale], nlast-delay[scale],
				q4_maxmod[i]-delay[scale], q4_maxmod[next]-delay[scale]);

		/* Ok, we found an interesting pair of max-mod, let's find
		 * a 0-crossing at 2^3. */
		offset_t p;
		
		tmpfound = detect_zero_crossing(3, q4_maxmod[i]-delay[scale]+delay[3], q4_maxmod[next]-delay[scale]+delay[3], &p);

		if (!tmpfound) { /* Zero not found at scale 2^3, trying again at scale 2^k */
			tmpfound = detect_zero_crossing(scale, q4_maxmod[i], q4_maxmod[next], &p);
		}

		if (tmpfound) {
			found = 1;
			if (out == 'p')
				add_point(p, p_wave, type_sec_peak);
			else
				add_point(p, t_wave, type_sec_peak);				
			/*if (q4_maxmod[i] < nfirst)
				nfirst = q4_maxmod[i];
			if (q4_maxmod[next] > nlast)
				nlast = q4_maxmod[next];*/

			int32_t thisdiff = (32*(int32_t)(data_abs(Q(scale, q4_maxmod[i])-Q(scale, q4_maxmod[next])))) /
				(q4_maxmod[next]-q4_maxmod[i]);

			dbg_printf("for this, diff="POT" > "POT"\n",
				thisdiff,
				maxdiff);

			if (out == 'p' || scale == 5) {
				if (thisdiff > maxdiff) {
					maxdiff = thisdiff;
					maxdiff_p = p;
					nfirst = q4_maxmod[i];
					nlast = q4_maxmod[next];
				}
			} else {
				if (5*thisdiff > 9*maxdiff) { // thisdiff/maxdiff > 10/5
					maxdiff = thisdiff;
					maxdiff_p = p;
					nfirst = q4_maxmod[i];
					nlast = q4_maxmod[next];
				}
#if 1
				else if (9*thisdiff > 5*maxdiff) {
					prefetch(q4_maxmod[next]+5-delay[4]+delay[5]);
					/* 2 diffs are very close, check values at scale 5 */
					data_t other = data_max(
						data_abs(Q(5, get_minmax(5, nfirst-5-delay[4]+delay[5],
								     nfirst+5-delay[4]+delay[5],
								     data_sign(Q(4, nfirst))))),
						data_abs(Q(5, get_minmax(5, nlast-5-delay[4]+delay[5],
								     nlast+5-delay[4]+delay[5],
								     data_sign(Q(4, nlast)))))
						);
					data_t this = data_max(
						data_abs(Q(5, get_minmax(5, q4_maxmod[i]-5-delay[4]+delay[5],
								     q4_maxmod[i]+5-delay[4]+delay[5],
								     data_sign(Q(4, q4_maxmod[i]))))),
						data_abs(Q(5, get_minmax(5, q4_maxmod[next]-5-delay[4]+delay[5],
								     q4_maxmod[next]+5-delay[4]+delay[5],
								     data_sign(Q(4, q4_maxmod[next])))))
						);
					dbg_printf("Special compare: this=%d, other=%d\n", this, other);
					if (this > other /*||
					    (this == other && nlast == q4_maxmod[i] &&
					     data_sign(Q(4, nfirst)) == data_sign(Q(4, q4_maxmod[next])) &&
					     data_sign(Q(4, nlast)) != data_sign(Q(4, q4_maxmod[next])))*/) { /* Bi-phasic T handling */
						maxdiff = thisdiff > maxdiff ? thisdiff: maxdiff;
						maxdiff_p = p;
						nfirst = q4_maxmod[i];
						nlast = q4_maxmod[next];
					}
//					else if (this == other) { /* We're probably sharing a peak at 2^5 */
//						nlast = q4_maxmod[next];
//					}
				}
#endif
			}
		}
	}
wave_out: ;
	offset_t p_onset, p_end;

	if (!found) {
		dbg_printf("%c_detect: pair not found (scale=%d)\n", out, scale);

		if (out == 't') {
			/* We got a big, isolated peak */
			offset_t p;
			offset_t rg_start, rg_stop;
			
#if 0
			if (data_abs(Q(scale, orig_init)) > data_abs(Q(scale, end))) {
/*				if (data_abs(Q(scale, orig_init)) < max)
				return 0;*/
				rg_start = orig_init;
				rg_stop = max_pos;
			} else {
/*				if (data_abs(Q(scale, end)) < max)
				return 0;*/
				rg_start = max_pos;
				rg_stop = end;
			}
#endif
			dbg_printf("%c_detect: maxpos="POT"/%d init="POT"/%d end="POT"/%d\n",
				   out, max_pos-delay[scale], Q(scale, max_pos),
				   orig_init-delay[scale], Q(scale, orig_init),
				   end-delay[scale], Q(scale, end));

			if (data_sign(Q(scale, orig_init)) != data_sign(Q(scale, max_pos))) {
				rg_start = orig_init;
				rg_stop = max_pos;
			} else if (data_sign(Q(scale, end)) != data_sign(Q(scale, max_pos))) {
				rg_start = max_pos;
				rg_stop = end;
			} else {
				return 0;
			}

			tmpfound = detect_zero_crossing(3, rg_start-delay[scale]+delay[3], rg_stop-delay[scale]+delay[3], &p);
			
			if (!tmpfound) { /* Zero not found at scale 2^3, trying again at scale 2^k */
				tmpfound = detect_zero_crossing(scale, rg_start, rg_stop, &p);
				if (!tmpfound)
					return 0;
			} else if (rg_start == orig_init) { /* Find the LAST zero-crossing */
				while (detect_zero_crossing(3, p+1+delay[3], rg_stop-delay[scale]+delay[3], &p)) {
					dbg_printf("%c_detect: p="POT"\n", out, p);
				}
			}
			
			maxdiff_p = p;
			nfirst = rg_start;
			nlast = rg_stop;
#if 0
			p_onset = detect_onset(scale, max_pos-1, 0, 1);
			maxdiff_p = max_pos-delay[scale];
			p_end = detect_end(scale, max_pos+1, 0);
#endif
		} else {
			return 0;
      		}
	}

	/* P/T onset and end */
	data_t xi_on  = data_abs(Q(scale, nfirst));
	data_t xi_end = data_abs(Q(scale, nlast));
	if (out == 'p') {
		xi_on  = xi_on  * P_XI_ON;
		xi_end = xi_end * P_XI_END;
	} else {
		xi_on  = xi_on  * T_XI_ON;
		xi_end = xi_end * T_XI_END;
	}
	
	dbg_printf("xi_on=%d, xi_end=%d\n", xi_on, xi_end);
	p_onset = detect_onset(scale, nfirst-1, xi_on, 1);
	p_end = detect_end(scale, nlast+1, xi_end, 1);

	if (out == 'p') {
		if (p_end > limit) {
			dbg_printf("WARNING: P end "POT" > qrs_onset "POT"\n", p_onset, limit);
		} else {
			add_point(maxdiff_p, p_wave, type_peak);
			add_point(p_onset, p_wave, type_onset);
			add_point(p_end, p_wave, type_end);
			if (pr1 == 0) {
				pr1 = this_r - (nfirst-delay[4]);
				pr2 = this_r - (nlast-delay[4]);
			} else {
				pr1 = (3*pr1 + (this_r - (nfirst-delay[4])))/4;
				pr2 = (3*pr2 + (this_r - (nlast-delay[4])))/4;
			}
		}
	} else {
		add_point(maxdiff_p, t_wave, type_peak);
		add_point(p_end, t_wave, type_end);
		this_t_end = p_end;
		if (p_onset < limit) {
			dbg_printf("WARNING: T onset "POT" < qrs_end "POT"\n", p_onset, limit);
		} else {
			add_point(p_onset, t_wave, type_onset);
		}
	}

	return 1;
}

/* NB: TODO: Check these values */
#define PQ_MAX_INTERVAL 100 // (340ms)
#define PQ_MIN_INTERVAL  1 // (  4ms)

#define P_COEFF_PR1 3/2
#define P_COEFF_PR2 3/5

#define P_COEFF_PR1_WIDE 2
#define P_COEFF_PR2_WIDE 1/2

static void p_detect(offset_t qrs_onset) {
	trace_fun();
	PROF_START;
	offset_t init1 = off_max(this_t_end, off_sub(qrs_onset, off_min(PQ_MAX_INTERVAL, rr/2)));
	offset_t end1 = off_sub(qrs_onset, PQ_MIN_INTERVAL);

	offset_t init = init1+delay[4];
	offset_t end = end1+delay[4];

	if (init1 < off_sub(offset, BUFFER_SIZE)) {
		dbg_printf("WARNING, I have to cut init1("POT") to "POT"...\n", init1, offset-BUFFER_SIZE+1);
		init1 = offset-BUFFER_SIZE+1;
	}

	if (pr1 > 0)
		init = off_max(off_sub(this_r, pr1*P_COEFF_PR1), init1)+delay[4];
	if (pr2 > 0)
		end = off_min(off_sub(this_r, pr2*P_COEFF_PR2), end1)+delay[4];

	if (init >= last_offset) {
		PROF_STOP;
		return;
	}

	if (end <= init) {
		PROF_STOP;
		dbg_printf("BAD in p_detect: end="POT" < init="POT" (init1="POT", end1="POT", rr="POT", qrs_onset="POT", pr1="POT", pr2="POT").\n",
			   end, init, init1, end1, rr, qrs_onset, pr1, pr2);
		return;
	}

	dbg_printf("p_detect between "POT" ("POT" "POT") and "POT" ("POT"-"POT")\n",
		init-delay[4], this_t_end, qrs_onset-PQ_MAX_INTERVAL, end-delay[4], this_r, pr2);

	if (pt_detect(init, end, qrs_onset, 4, epsilon_p, 'p')) {
		PROF_STOP;
		return;
	}

	compute_epsilon_pt_5();
	dbg_printf("p_detect: Retrying at scale 5\n");
	if (pt_detect(init-delay[4]+delay[5], end-delay[4]+delay[5], qrs_onset, 5,
		      epsilon_p_5, 'p')) {
		PROF_STOP;
		return;
	}

	if (pr1 == 0) {
		PROF_STOP;
		return;
	}

	init = off_max(off_sub(this_r, pr1*P_COEFF_PR1_WIDE), init1)+delay[4];
	end = off_min(off_sub(this_r, pr2*P_COEFF_PR2_WIDE), end1)+delay[4];
	
	dbg_printf("p_detect: Retrying with wider area of research\n");
	if (pt_detect(init, end, qrs_onset, 4, epsilon_p, 'p')) {
		PROF_STOP;
		return;
	}

	PROF_STOP;
}

static const offset_t t_end_rr[] =
	{375, 300, 250, 214, 188, 167, 150, 125, 100,  83,  75};
/* These are normal limits */
/*
const double t_end_qtc_min[] =
	{.41, .38, .35, .33, .32, .30, .28, .26, .23, .21, .20};
const double t_end_qtc_max[] =
	{.51, .46, .43, .41, .39, .36, .34, .32, .28, .25, .24};
*/
/* These are useful values for below: i.e.
   qtc_min = 250*qtc_min/sqrt(250)*128
   qtc_max = 250*qtc_max/sqrt(250)*128
*/
static const int32_t t_end_qtc_min[] = { 830, 769, 708, 668, 648, 607, 567, 526, 465, 425, 405};
static const int32_t t_end_qtc_max[] = {1032, 931, 870, 830, 789, 729, 688, 648, 567, 506, 486};

#define QTC_MAX_RATIO 7/5
#define QTC_MAX_RATIO_BIS 7/5
#define QTC_MAX_RATIO_5 7/5

static void t_detect(offset_t qrs_onset, offset_t qrs_end) {
	trace_fun();
	if (n_valid <= 1) {
		/* Do not detect T wave on the first beat, since it may detect
		 * what should be a QRS. */
		return;
	}

	PROF_START;
	unsigned int i;
	for (i = 0; i < (sizeof(t_end_rr)/sizeof(offset_t))-1; i++) {
		if (rr >= t_end_rr[i]) {
			break;
		}
	}
	/*trace_printf("t_detect, rr="POT", i=%d using rr="POT", qtc="POT"/"POT" last_offset="POT"\r\n",
	  rr, i, t_end_rr[i], t_end_qtc_min[i], t_end_qtc_max[i], last_offset);*/

	dbg_printf("t_detect, rr="POT", i=%d using rr="POT", qtc="POT"/"POT"\n",
		   rr, i, t_end_rr[i], t_end_qtc_min[i], t_end_qtc_max[i]);

	int32_t srr = isqrt32(rr);
	/* Whatever happens, we only start detecting the T 40ms after QRS_end */
	offset_t init = qrs_end+10+delay[4];
	//init = off_max(qrs_end+15, qrs_onset+((t_end_qtc_min[i]*srr*QTC_MIN_RATIO+64)/128))+delay[4];
	offset_t end = off_min(last_offset, qrs_onset+((t_end_qtc_max[i]*srr*QTC_MAX_RATIO+64)/128)+delay[4]);

	if (init >= last_offset) {
		PROF_STOP;
		return;
	}

	trace_printf("t_detect between ("POT"/"POT") "POT" and "POT" (%d, srr=%ld, i=%d)\r\n",
		     qrs_onset, qrs_end, init-delay[4], end-delay[4], epsilon_t, srr, i);

	dbg_printf("t_detect between ("POT") "POT" and "POT" (%d)\n",
				qrs_end, init-delay[4], end-delay[4], epsilon_t);

	// BUG remover... It seems that the end values goes crazy sometimes, FIXME!!!
	if (end > init+rr) {
		end = init+rr;
	}

	if (!pt_detect(init, end, qrs_end, 4, epsilon_t, 't')) {
		trace_printf("T failed at 4, retry at 5 (eps)\n");
		compute_epsilon_pt_5();
		trace_printf("T failed at 4, retry at 5 (pt)\n");
		dbg_printf("t_detect: Retrying at scale 5\n");
		init = qrs_end+10+delay[5];
		end = off_min(last_offset, qrs_onset+((t_end_qtc_max[i]*srr*QTC_MAX_RATIO_5+64)/128)+delay[5]);

		pt_detect(init-delay[4]+delay[5], end-delay[4]+delay[5], qrs_end, 5, epsilon_t_5, 't');
	}
	trace_printf("T done\r\n");
	PROF_STOP;
}

/* sel102 has really wide QRS complexes... The standard value should be 120ms,
 * but I took 240ms. */
#define LI_4_NEIGHBOUR 60 // (160 [ms]). Also used for Gamma.
#define LI_4_NEIGHBOUR_BEFORE 20 // (80 [ms]). Also used for Gamma.
//#define LI_4_NEIGHBOUR_CLOSE 20 // (80 [ms]). Also used for Gamma.

#define GAMMA_QRS_PRE  3/50 // 0.06
#define GAMMA_QRS_POST_1 1/5 // 0.20
//#define GAMMA_QRS_POST_2 1/5 // 0.2

/* 2) [Martinez] */
static void qrs_detect_waves(offset_t nr) {
	trace_fun();
	PROF_START;
	dbg_printf("qrs_detect_waves ("POT", "POT", "POT")\n", nr, ok_maxmin[0][2],ok_maxmin[1][2] );
	data_t max = data_max(data_abs(Q(2, ok_maxmin[0][2])), data_abs(Q(2, ok_maxmin[1][2])));
	data_t thres = max*GAMMA_QRS_PRE; /* gamma_qrs_(pre/post) */
	offset_t init = off_sub(nr+delay[2], LI_4_NEIGHBOUR_BEFORE);
	offset_t end = init+LI_4_NEIGHBOUR; //-LI_4_NEIGHBOUR_BEFORE;

/* npre: maxmod at 2^2 before R, npost: after R */
/*	int npre = ok_maxmin[0][2];
	int npost = ok_maxmin[1][2];*/

	//int nfirst = nr+delay[2], nlast = nr+delay[2];

#define Q2_MAXMOD_NR_MAX 16
	offset_t q2_maxmod[Q2_MAXMOD_NR_MAX];
	int q2_maxmod_nr = 0;
	int i;

	dbg_printf("qrs_detect ("POT", "POT", "POT")\n", nr, init-delay[2], end-delay[2]);

	if (init < delay[4])
		init += delay[4];

	/* Store potential maxima (need postprocessing once gamma is computed) and get the max value */
	/* NB: Martinez is unclear. Is gamma_QRS_pre computed only on values before the R? */

	while (1) {
		if (!findmodmax(2, init, end, thres, &init))
			break;

		q2_maxmod[q2_maxmod_nr++] = init;
		if (q2_maxmod_nr == Q2_MAXMOD_NR_MAX) {
			dbg_printf("qrs_detect: BUG (q2_maxmod overflow)\n");
			q2_maxmod_nr--;
		}
		if (data_abs(Q(2, init)) > max) {
			max = data_abs(Q(2, init));
			thres = max*GAMMA_QRS_PRE;
		}

		dbg_printf("qrs_detect: modmax@"POT" (%d, %d)\n", init-delay[2], q2_maxmod_nr-1, Q(2, q2_maxmod[q2_maxmod_nr-1]));
	}

	/* [Martinez] says only QRS complexes with 3 or less waves are considered, this
	 * leaves only 2 waves apart from the R. We take the closest ones to the R.
	 * (TODO: Is this really clever? What about taking the maximum difference in the 2 modmax?) */
	offset_t waves[2]; /* Position of the waves. */
	offset_t peaks[2][2]; /* Position of the peaks associated with the waves. */
	int nr_waves = 0;

	thres = max*GAMMA_QRS_PRE; /* NB: My own tuning */
	//thres = max*9/100;
	for (i = 0; i < q2_maxmod_nr-1 && q2_maxmod[i] < nr+delay[2]; i++) {
		int next = i+1;

		dbg_printf("qrs_detect_pre: trying %d-%d (%d %d (%d))\n", i, next,
				Q(2, q2_maxmod[i]), Q(2, q2_maxmod[next]), thres);

		if (data_abs(Q(2, q2_maxmod[i])) < thres)
			continue;

		while (data_abs(Q(2, q2_maxmod[next])) < thres) {
			next++;
			if (q2_maxmod[next] > nr+delay[2] || next >= q2_maxmod_nr)
				goto pre_out;
			dbg_printf("qrs_detect_pre: trying %d-%d (%d %d)\n", i, next,
				Q(2, q2_maxmod[i]), Q(2, q2_maxmod[next]));
		}

		if (data_sign(Q(2, q2_maxmod[i])) == data_sign(Q(2, q2_maxmod[next])))
			continue;

		/* Ok, we found an interesting pair of max-mod, let's find
		 * a 0-crossing at 2^1. */
		offset_t qrs;
		
		if (detect_zero_crossing(1, q2_maxmod[i]-delay[2]+delay[1], q2_maxmod[next]-delay[2]+delay[1], &qrs)) {
			int j;
			if (nr_waves < 2) {
				waves[nr_waves] = qrs;
				peaks[nr_waves][0] = q2_maxmod[i];
				peaks[nr_waves][1] = q2_maxmod[next];
				nr_waves++;
				dbg_printf("qrs_detect_pre: added wave "POT" (R="POT"), peak="POT"\n", qrs, nr, q2_maxmod[i]-delay[2]);
			}
			else {
				for (j = 0; j < nr_waves; j++) {
					if (data_abs(Q(2, q2_maxmod[i])-Q(2, q2_maxmod[next]))
							> data_abs(Q(2, peaks[j][0])-Q(2, peaks[j][1]))
								&& qrs != nr) {
						dbg_printf("qrs_detect_pre: replaced wave "POT" by "POT" (R="POT"), peak="POT"\n",
							waves[j], qrs, nr, q2_maxmod[i]);
						waves[j] = qrs;
						peaks[j][0] = q2_maxmod[i];
						peaks[j][1] = q2_maxmod[next];
						break;
					}
				}
			}
		}
	}
pre_out:
	
	dbg_printf("qrs_detect: qrs_post\n");
	thres = max*GAMMA_QRS_POST_1;
	for (; i < q2_maxmod_nr-1; i++) {
		int next = i+1;

		if (q2_maxmod[next] < nr+delay[2]) continue;

		dbg_printf("qrs_detect_post: trying %d-%d (%d %d (%d))\n", i, next,
				Q(2, q2_maxmod[i]), Q(2, q2_maxmod[next]), thres);

/*		if (q2_maxmod[i]-delay[2]-nr > LI_4_NEIGHBOUR_CLOSE)
		thres = max*GAMMA_QRS_POST_2;*/

		if (data_abs(Q(2, q2_maxmod[i])) < thres)
			continue;

/*		if (q2_maxmod[next]-delay[2]-nr > LI_4_NEIGHBOUR_CLOSE)
		thres = max*GAMMA_QRS_POST_2;*/
		while (data_abs(Q(2, q2_maxmod[next])) < thres) {
			next++;
			if (next >= q2_maxmod_nr)
				goto post_out;
			dbg_printf("qrs_detect_post: trying %d-%d (%d %d)\n", i, next,
					Q(2, q2_maxmod[i]), Q(2, q2_maxmod[next]));
			/*if (q2_maxmod[next]-delay[2]-nr > LI_4_NEIGHBOUR_CLOSE)
			  thres = max*GAMMA_QRS_POST_2;*/
		}

		if (data_sign(Q(2, q2_maxmod[i])) == data_sign(Q(2, q2_maxmod[next])))
			continue;

		/* Ok, we found an interesting pair of max-mod, let's find
		 * a 0-crossing at 2^1. */
		offset_t qrs;

		if (detect_zero_crossing(1, q2_maxmod[i]-delay[2]+delay[1], q2_maxmod[next]-delay[2]+delay[1], &qrs)) {
			int j;
			if (nr_waves < 2) {
				waves[nr_waves] = qrs;
				peaks[nr_waves][0] = q2_maxmod[i];
				peaks[nr_waves][1] = q2_maxmod[next];
				nr_waves++;
				dbg_printf("qrs_detect_post: added wave "POT" (R="POT"), peak="POT"\n", qrs, nr, q2_maxmod[next]-delay[2]);
			}
			else {
				for (j = 0; j < nr_waves; j++) {
					if (data_abs(Q(2, q2_maxmod[i])-Q(2, q2_maxmod[next]))
							> data_abs(Q(2, peaks[j][0])-Q(2, peaks[j][1]))
								&& qrs != nr) {
						dbg_printf("qrs_detect_post: replaced wave "POT" by "POT" (R="POT"), peak="POT"\n",
							waves[j], qrs, nr, q2_maxmod[next]);
						waves[j] = qrs;
						peaks[j][0] = q2_maxmod[i];
						peaks[j][1] = q2_maxmod[next];
						break;
					}
				}
			}
		}
	}
	/* TODO: Here, we could find out the QRS morphology: QRS, RSR', QR, RS... */

post_out: ;
	offset_t nfirst = ok_maxmin[0][2];
	offset_t nlast = ok_maxmin[1][2];
	offset_t first_wave = nr+delay[2];
	offset_t last_wave = nr+delay[2];
	for (i = 0; i < nr_waves; i++) {
		int j;
		add_point(waves[i], qrs_wave, type_sec_peak);
		for (j = 0; j < 2; j++) {
			if (peaks[i][j] < nfirst)
				nfirst = peaks[i][j];
			else if (peaks[i][j] > nlast)
				nlast = peaks[i][j];
		}
		if (waves[i] < first_wave)
			first_wave = waves[i];
		else if (waves[i] > last_wave)
			last_wave = waves[i];
	}

	/* QRS onset and end */
	offset_t qrs_onset, qrs_end;

#define MARTINEZ_QRS_ONSET_END 0
#if MARTINEZ_QRS_ONSET_END
	data_t q2_nfirst = Q(2)[nfirst];
	data_t q2_nlast = Q(2)[nlast];
	data_t xi_on = (q2_nfirst > 0 ?  0.05*q2_nfirst : -0.07*q2_nfirst);
	data_t xi_end = (q2_nlast  > 0 ? 0.125*q2_nlast  : -0.71*q2_nlast);

	qrs_onset = detect_onset(2, nfirst-1, xi_on, 1);
	qrs_end = detect_end(2, nlast+1, xi_end);
#else
	data_t xi_on;
	data_t xi_end;

	offset_t max_q4_onset = get_minmax(4, nfirst-delay[2]+delay[4]-10, nr+delay[4], 1);
	xi_on = data_abs(Q(4, max_q4_onset))*1/4; /* 0.25 */

	offset_t onset_start_q4 = off_sub(nfirst-delay[2]+delay[4], 2); /* 8ms before the first wave */
	dbg_printf("max_q4_onset="POT", onset_start_q4="POT"\n", max_q4_onset-delay[4], onset_start_q4-delay[4]);

	dbg_printf("detecting onset from "POT"\n", onset_start_q4-delay[4]);
	qrs_onset = detect_onset(4, onset_start_q4, xi_on, 0);

	dbg_printf("getting max from "POT" to "POT"\n", nlast-delay[2]+delay[4]+1, nlast-delay[2]+delay[4]+10);
	offset_t end_start_q4 = get_minmax(4, nlast-delay[2]+delay[4], nlast-delay[2]+delay[4]+20, 1);
	xi_end = (int32_t)data_abs(Q(4, end_start_q4))*7/10; /* 0.70 */

	dbg_printf("detecting end from "POT" (Q4:%d)\n", end_start_q4-delay[4], data_abs(Q(4, end_start_q4)));
	qrs_end = detect_end(4, end_start_q4, xi_end, 0);
#endif

	/* De-bias */
/*	qrs_onset -= 1;
	qrs_end -= 1;*/

	add_point(qrs_onset, qrs_wave, type_onset);
	add_point(qrs_end, qrs_wave, type_end);

	this_qrs_end = qrs_end;

#undef Q2_MAXMOD_NR_MAX

	PROF_STOP;
	dbg_printf("compute_epsilon_pt\n");
	compute_epsilon_pt();
	dbg_printf("p_detect\n");
	p_detect(qrs_onset);
	dbg_printf("t_detect\n");
	t_detect(qrs_onset, qrs_end);
}

/* This function detects the R wave. It is called only when no modmax has
 * been encountered for at least 120ms */
static int check_r_wave() {
	trace_fun();
	offset_t r;

	dbg_printf("R-Wave detection (%d, "POT"-"POT")...\n", ok_maxmin_valid, ok_maxmin[0][1], ok_maxmin[1][1]);
	if (ok_maxmin_valid == 2) { /* 2 points */
		if (ok_maxmin[1][1]-ok_maxmin[0][1] > LI_4_NEIGHBOUR) {
			/* min-max interval too wide, should not happen */
			dbg_printf("Too wide min-max interval, should not happen...\n");
			ok_maxmin_valid = 0;
			return 0;
		}

		/* 5) [Li] */
		if (detect_zero_crossing(1, ok_maxmin[0][1], ok_maxmin[1][1], &r)) {
			add_point(r, qrs_wave, type_peak);
			add_r(r);
			qrs_detect_waves(r);
			ok_maxmin_valid = 0;
			return 1;
		}
		dbg_printf("R not found??\n");
	} else {
		/* Means an isolated modmax/min has been found */
	}

	ok_maxmin_valid = 0;
	return 0;
}

/* 4) [Li] */
static void add_modmax(offset_t* modmax, int searchback) {
	trace_fun();
	int mmsign;

	if (searchback) {
		mmsign = data_sign(Q(3, modmax[3]));
	}
	else {
		if (Q(1, modmax[1]) > 0 && Q(2, modmax[2]) > 0 && Q(3, modmax[3]) > 0 && Q(4, modmax[4]) > 0) {
			mmsign = 1;
		} else if (Q(1, modmax[1]) < 0 && Q(2, modmax[2]) < 0 && Q(3, modmax[3]) < 0 && Q(4, modmax[4]) < 0) {
			mmsign = 0;
    		} else {
			dbg_printf("weird modmax (not constant sign): "POT"/%d "POT"/%d "POT"/%d "POT"/%d\n", 
				modmax[1]-delay[1], Q(1, modmax[1]),
				modmax[2]-delay[2], Q(2, modmax[2]),
				modmax[3]-delay[3], Q(3, modmax[3]),
				searchback ? 0 : modmax[4]-delay[4], searchback ? 0 : Q(4, modmax[4])
				);
			return;
		}
	}

#define MOVE_MAXMIN(newpos, data) memcpy(&ok_maxmin[newpos], data, sizeof(ok_maxmin[newpos]));
	if (ok_maxmin_valid < 2) {
		/* Enough space, put it in the list */
		MOVE_MAXMIN(ok_maxmin_valid, modmax);
		ok_maxmin_valid++;
	} else { /* ok_maxmin_valid == 2 */
		if (data_sign(Q(3, ok_maxmin[0][3])) == mmsign) {
			if (data_sign(Q(3, ok_maxmin[1][3])) == mmsign) {
				/* Same sign as element 0 and 1, eliminate 0. */
				dbg_printf("modmax eliminated: "POT" (sign)\n", ok_maxmin[0][3]-delay[3]);
				MOVE_MAXMIN(0, &ok_maxmin[1]);
				MOVE_MAXMIN(1, modmax);
			} else {
				/* Same sign as element 0. (1 is the "pivot") */
				data_t a1l1 = data_abs(Q(3, ok_maxmin[0][3]))/(ok_maxmin[1][3]-ok_maxmin[0][3]);
				data_t a2l2 = data_abs(Q(3, modmax[3]))/(modmax[3]-ok_maxmin[1][3]);

				if (a1l1*5 > a2l2*6) { /* 1.2 */
					dbg_printf("modmax eliminated: "POT" (1.2 rule)\n", modmax[3]-delay[3]);
					/* modmax is redundant */
				} else if (a2l2*5 > a1l1*6) {
					/* 0 is redundant */
					dbg_printf("modmax eliminated: "POT" (sign)\n", ok_maxmin[0][3]-delay[3]);
					MOVE_MAXMIN(0, &ok_maxmin[1]);
					MOVE_MAXMIN(1, modmax);
				} else {
					dbg_printf("modmax eliminated: "POT" (follow)\n", modmax[3]-delay[3]);
					/* modmax is redundant (following the peak) */
				}
			}
		} else { /* Sign different from 0 */
			if (data_sign(Q(3, ok_maxmin[1][3])) == mmsign) {
				/* 1 and modmax have the same sign, one of them is redundant */
				if (modmax[1]-ok_maxmin[1][1] > LI_4_NEIGHBOUR) {
					dbg_printf("modmax eliminated: "POT" (isolated)\n", modmax[1]-delay[1]);
					/* 0-current interval > 120ms => modmax is isolated */
					return;
				}
				
				data_t a1l1 = data_abs(Q(3, ok_maxmin[1][3]))/(ok_maxmin[1][3]-ok_maxmin[0][3]);
				data_t a2l2 = data_abs(Q(3, modmax[3]))/(modmax[3]-ok_maxmin[0][3]);

				if (a1l1*5 > 6*a2l2) {
					dbg_printf("modmax eliminated: "POT" (1.2 rule)\n", modmax[3]-delay[3]);
					/* modmax is redundant */
				} else if (a2l2*5 > 6*a1l1) {
					/* 1 is redundant */
					dbg_printf("modmax eliminated: "POT" (sign)\n", ok_maxmin[1][3]-delay[3]);
					MOVE_MAXMIN(1, modmax);
				} else {
					dbg_printf("modmax eliminated: "POT" (furthest)\n", modmax[3]-delay[3]);
					/* modmax is redundant (same side, rules 3 says the furthest is redundant) */
				}
			}
			else {
				/* 0 and 1 have the same sign, one of them is redundant */
				if (modmax[1]-ok_maxmin[0][1] > LI_4_NEIGHBOUR) {
					/* 0-current interval > 120ms => 0 is isolated */
					MOVE_MAXMIN(0, &ok_maxmin[1]);
					MOVE_MAXMIN(1, modmax);
					return;
				}
				
				data_t a1l1 = data_abs(Q(3, ok_maxmin[0][3]))/(modmax[3]-ok_maxmin[0][3]);
				data_t a2l2 = data_abs(Q(3, ok_maxmin[1][3]))/(modmax[3]-ok_maxmin[1][3]);

				if (a1l1*5 > 6*a2l2) {
					/* 1 is redundant */
					MOVE_MAXMIN(1, modmax);
				} else if (a2l2*5 > 6*a1l1) {
					/* 0 is redundant */
					MOVE_MAXMIN(0, &ok_maxmin[1]);
					MOVE_MAXMIN(1, modmax);
				} else {
					/* 0 is redundant (same side, rules 3 says the furthest is redundant) */
					MOVE_MAXMIN(0, &ok_maxmin[1]);
					MOVE_MAXMIN(1, modmax);
				}
			}
		}
	}
#undef MOVE_MAXMIN
}

/* 2) [Li] */
#define LI_2_NEIGHBOUR 10
static void li2_iterate(int scale, offset_t* modmax, int searchback) {
//	trace_fun();
	PROF_START;
	do { /* Big loop to ensure tail recursion optimization */
		offset_t init = modmax[scale+1]-delay[scale+1]+delay[scale];
		offset_t tmp = off_sub(init, LI_2_NEIGHBOUR);
		data_t threshold = EPS(scale);
		int found = 0;

		while (1) {
			if (!findmodmax(scale, tmp, init+LI_2_NEIGHBOUR, threshold, &tmp))
				break;
			
			dbg_printf("li2_iterate: q%d-modmax@"POT"\n", scale, tmp-delay[scale]);
			
			/* TODO: NB: Modify this, if closer, a modmax < 1.2*current is good */
			
			/* NB: This seems not to work well... */
			/* From now we are only interested in modmax > 1.2 the current one */
			//threshold = 1.2*modmax[scale];
			
			/* NB: I take the nearest one instead */
			if (!found || (off_diff(modmax[scale], init) > off_diff(tmp, init)))
				modmax[scale] = tmp;
			
			found = 1;
			
			/*dbg_printf("modmax-q%d@%d: %lf (T=%lf, I=%d)\n", scale,
			  modmax[scale]-delay[scale], Q(scale)[modmax[scale]], threshold, init-delay[scale]);*/
		}
		
		if (found) {
			//dbg_printf("modmax-q%d@%d: %lf\n", scale, modmax[scale]-delay[scale], Q(scale)[modmax[scale]]);
			if (scale > 1) {
				scale = scale-1;
				continue;
			}
			else {
				/* 3) [Li] */
				/* NB: Alpha check seems weird (like in 10677 of sel100).. I ignore it, as [Martinez]... */
				/*double alpha[4];
				  int i;*/
				
				dbg_printf("----\nmodmax: "POT"/%d "POT"/%d "POT"/%d "POT"/%d\n", 
					   modmax[1]-delay[1], Q(1, modmax[1]),
					   modmax[2]-delay[2], Q(2, modmax[2]),
					   modmax[3]-delay[3], Q(3, modmax[3]),
					   searchback ? 0 : modmax[4]-delay[4], searchback ? 0 : Q(4, modmax[4])
					);
				
				/*for (i = 1; i < 4; i++) {
				  alpha[i] = log(data_abs(q[i+1][modmax[i+1]]))-log(data_abs(q[i][modmax[i]]));
				  }*/
				
				/*dbg_printf("alpha: %lf %lf %lf\n", 
						alpha[1], alpha[2], alpha[3]);*/

				add_modmax(modmax, searchback);
			}
		}
		PROF_STOP;
		return; /* We come here only is continue has not been called */
	} while(1);
}

static int li2_searchback(offset_t until) {
	trace_fun();
	PROF_START;
	offset_t modmax[5];

	dbg_printf("li_2: Search back!\n");

	modmax[3] = off_max(this_r + rr/2 + delay[3], off_sub(offset, BUFFER_SIZE)); /* Start from half the period */
	until = until + delay[3];

	dbg_printf("li_2: From "POT" to "POT" ("POT" "POT")\n", modmax[3]-delay[3], until, this_r, last_r);

	while (1) {
		if (!findmodmax(3, modmax[3], until, EPS(3), &modmax[3])) {
			if (!check_r_wave()) {
				searchback_r = off_max(searchback_r, this_r) + rr;
				/* Update this_r and last_r even if we didn't find any... */
				dbg_printf("li_2: Search back is over ("POT" "POT").\n", this_r, last_r);
				PROF_STOP;
				return 0;
			}
			else {
				dbg_printf("li_2: Search back is over (ok!).\n");
				PROF_STOP;
				return 1;
			}
		}

		dbg_printf("modmax-q3-sb@"POT": %d\n", modmax[3]-delay[3], Q(3, modmax[3]));

		modmax[1] = modmax[3]-delay[3]+delay[1];
		modmax[2] = modmax[3]-delay[3]+delay[2];

		add_modmax(modmax, 1);

		/* Ok, we found a mod max at scale 3, find some at scale 2 in
		 * the "neighbourhood" [Li], e.g. 10 samples. */
		//li2_iterate(2, modmax, 1);
	}
}

#define LI2_BLANKING 60 // 240ms
static void li2() {
	trace_fun();
	offset_t modmax[5];
	offset_t i, j;
	offset_t until;
	offset_t searchback = 0;
	dbg_printf("li2\n");

	ok_maxmin_valid = 0;
	for (j = 0; j < 2; j++) {
		for (i = 1; i < 5; i++) {
			ok_maxmin[j][i] = 0;
		}
	}

	modmax[4] = delay[4];
	while (1) {
		until = last_offset;

		if (n_valid > 1) { /* We need 2 R before thinking about searching back. */
			searchback = off_min(off_max(this_r, searchback_r)+rr*3/2, until);
		} else {
			searchback = off_min(off_max(this_r, searchback_r)+300, until);
		}
		
		until = off_min(until, searchback);

		/* If a maxmin is in the list, call check_r_wave after 120ms without any point. */
		if (ok_maxmin_valid > 0) {
			until = off_min(until, ok_maxmin[ok_maxmin_valid-1][1]+LI_4_NEIGHBOUR-delay[1]+delay[4]);
		}

 		dbg_printf("li_2: @"POT", -> "POT" ("POT") (eps=%d %d %d %d)\n", modmax[4], until, searchback, EPS(1), EPS(2), EPS(3), EPS(4));

		if (!findmodmax(4, modmax[4], until, EPS(4), &modmax[4])) {
			/* No peak for 120ms (or we hit the search back limit), check for an R wave. */
			int r_ok = check_r_wave();

			if (!r_ok && until == searchback && until != last_offset) {
				dbg_printf("HERE; I SEARCH BACK! (this="POT" last="POT" rr="POT" until="POT" sb="POT" sb_r="POT")\n",
					this_r, last_r, rr, until, searchback, searchback_r);
				/* We hit the search back limit */
				li2_searchback(until-rr/2);
			}
		     
			if (until == last_offset)
				break;
			else {
				/* Blanking */
				if (r_ok)
					modmax[4] = off_max(until, off_max(this_qrs_end+LI2_BLANKING, this_t_end)+delay[4]);
				else
					modmax[4] = until;
				continue;
			}
		}

		dbg_printf("li_2: =>"POT" ("POT" %d)\n", modmax[4], until-this_r, (data_t)((this_r-last_r)*1.5));

		dbg_printf("li_2: Last R ("POT" "POT", rr="POT").\n", this_r, last_r, rr);

		dbg_printf("modmax-q4@"POT": %d\n", modmax[4]-delay[4], Q(4, modmax[4]));

		/* Ok, we found a mod max at scale 4, find some at scale 3 in
		 * the "neighbourhood" [Li], e.g. 10 samples. */
		li2_iterate(3, modmax, 0);
	}
}

#if ON_PC
/* Arguments:
 *  input_file1 input_file2 [anot_file start_time]
 * Where input_file1 is the first input file (- for stdin).
 * input_file2 is the second input file (0 for none used)
 * anot_file is the annotation file (if validation is requested)
 * start_time is the starting time in the annotation file (if validation is requested)
 */
int main(int argc, char** argv) {
	int i;
  
	if (argc != 4 && argc != 6) {
		printf("Invalid number of arguments (%d).\n", argc);
		return 1;
	}

	multiplier = 32768/atoi(argv[3]);
	if (multiplier > DATA_MULTIPLIER)
		multiplier = DATA_MULTIPLIER;
	fprintf(stderr, "Using multiplier %d\n", multiplier);

	for (i = 0; i < 2; i++) {
		if (!strcmp(argv[i+1], "-"))
			datafile = stdin;
		else if (!strcmp(argv[i+1], "0"))
			break;
		else
			datafile = fopen(argv[i+1], "r");

		if (!datafile) {
			perror("Cannot open data file.");
			return 1;
		}

		reset_variables();
		set_lead(i);

		fprintf(stderr, "Fill in...\n");
		fill_in();

		/* Delineation */
		fprintf(stderr, "li2\n");
		li2();
	}

	dump_points("r", qrs_wave, type_peak);
	dump_points("qrs", qrs_wave, type_sec_peak);
	dump_points("qrs_onset", qrs_wave, type_onset);
	dump_points("qrs_end", qrs_wave, type_end);
	dump_points("p", p_wave, type_peak);
	dump_points("psec", p_wave, type_sec_peak);
	dump_points("p_onset", p_wave, type_onset);
	dump_points("p_end", p_wave, type_end);
	dump_points("t", t_wave, type_peak);
	dump_points("tsec", t_wave, type_sec_peak);
	dump_points("t_onset", t_wave, type_onset);
	dump_points("t_end", t_wave, type_end);

	if (argc == 6) {
		read_validation(argv[4], atoi(argv[5]));
		dump_val();
		check_validation();
	}

	return 0;
}
#else
static void dumpstack();

uint64_t localtime = 0;

int putchar_uart1(int c) {
	while (!(IFG2 & UTXIFG1));
	TXBUF1 = (unsigned char)c;
	return 0;
}

#if !SERIAL_DATA && !STATIC_DATA
static int multiplier = 10;
interrupt (TIMERA0_VECTOR) timer2( void ) {
	static int c = 0;

        if (IFG2 & URXIFG1) {
		unsigned int data = U1RXBUF;
		if ((data & 0xC0) == 0x80) {
			/* Channel */
			ADC12CTL0 &= ~ENC;
			ADC12MCTL0 = (data & 0x0F) | SREF_1;
			ADC12CTL0 |= ENC;
			putchar_uart1(0xCA);
			putchar_uart1(ADC12MCTL0 & 0x0F);
		}
		if ((data & 0xC0) == 0x40) {
			/* Multiplier */
			multiplier = data & 0x3F;
			putchar_uart1(0x88);
			putchar_uart1(multiplier);
		}
		if ((data & 0xC0) == 0xC0) {
			/* Software reset: write invalid data in Watchdog register */
			WDTCTL = 0xBEEF;			
		}
	}

	if ((adc_offset & 0xFF) == 0x00) {
		P4OUT &= ~(1 << c);
		c++;
		if (c == 3) c = 0;
		P4OUT |= (1 << c);
		putchar_uart1(0xCC); /* Print current offset (4 bytes) */
		putchar_uart1((adc_offset >> 24) & 0xff);
		putchar_uart1((adc_offset >> 16) & 0xff);
		putchar_uart1((adc_offset >>  8) & 0xff);
		putchar_uart1((adc_offset >>  0) & 0xff);
		putchar_uart1(0xFB); /* Print HBR, (2 bytes) */
		putchar_uart1((rr >> 8) & 0xFF);
		putchar_uart1(rr & 0xFF);
	}

	if ((adc_offset & 0x3FF) == 0x00) {
		putchar_uart1(0xCA);
		putchar_uart1(ADC12MCTL0 & 0x0F);
		putchar_uart1(0x88);
		putchar_uart1(multiplier);
		//dumpstack();
	}

	unsigned int data = ADC12MEM0*multiplier;
	putchar_uart1(0xDA); /* Send data: 2 bytes */
	putchar_uart1(data >> 8);
	putchar_uart1(data & 0xFF);
	IN(adc_offset) = data;
	adc_offset++;

	ADC12CTL0 |= ADC12SC; /* Start conversion */

	localtime++;
}
#else
interrupt(TIMERA1_VECTOR) timer1() {
       /* Timer A overflow */
       if (TAIV == 0x0a) {
               localtime++;
       }
}
#endif

#if SERIAL_DATA
static INLINE void resumetiming() {
	TACTL |= MC_2;
}

static INLINE void stoptiming() {
	TACTL &= ~MC_3; /* Stop the timer */
}
#else
static INLINE void resumetiming() {}

static INLINE void stoptiming() {}
#endif

#if SERIAL_DATA || STATIC_DATA
static void readuart() {
	int cnt = 0;
	char* cdata = (char*)&data;
	int16_t csum = 0;
	volatile long int i;

	stoptiming();

	putchar(ASK);
	while (1) {
		i = 0;
		while (!(IFG1 & URXIFG0) && (i < 100000L))
			i++;
		if (!(IFG1 & URXIFG0)) {
			putchar(NACK);
			putchar(START);
			printf("Timeout1 (%d)", cnt);
			putchar(END);
			putchar(ASK);
			cnt = 0;
			continue;
		}
		*cdata++ = U0RXBUF;
		i = 0;
		while (!(IFG1 & URXIFG0) && (i < 100000L))
			i++;
		if (!(IFG1 & URXIFG0)) {
			putchar(NACK);
			putchar(START);
			printf("Timeout2 (%d)", cnt);
			putchar(END);
			putchar(ASK);
			cnt = 0;
			continue;
		}
		*cdata++ = U0RXBUF;

		if (cnt >= 2) {
			csum = csum ^ data.buffer[cnt-2];
		}

		cnt++;

		if (cnt >= 1 && cnt == data.length) { /* Length */
			if (csum == data.csum) {
				putchar(ACK);
				resumetiming();
				return;
			}
			else {
				putchar(NACK);
				putchar(START);
				printf("Checksum error (%x %x)\n", csum, data.csum);
				putchar(END);
				putchar(ASK);
				cnt = 0;
			}
		}
	}
}
#endif

int putchar(int c)
{
	while (!(IFG1 & UTXIFG0));                // USART0 TX buffer ready?
	TXBUF0 = (unsigned char)c;
	return 0;
}

int dummy;

void stopme() {
	while(1) {
		dummy++;
	}
}

int counter = 0;

#if STATIC_DATA
void add_point(offset_t offset, enum wave wave, enum type type) {

}
#else
void add_point(offset_t offset, enum wave wave, enum type type) {
	stoptiming();

#if SERIAL_DATA
	char w = '?', t = '?';

	switch (wave) {
	case qrs_wave: w = 'R'; break;
	case p_wave:   w = 'P'; break;
	case t_wave:   w = 'T'; break;
	default:       w = '?';
	}

	switch (type) {
	case type_onset:    t = 'o'; break;
	case type_peak:     t = 'p'; break;
	case type_end:      t = 'e'; break;
	case type_sec_peak: t = 's'; break;
	default:            t = '!';
	}

	putchar(START);
	printf("P%c%c:%lu\r\n", w, t, offset);
	putchar(END);
	if (wave == qrs_wave && type == type_peak) {
		counter++;
		P4OUT = (P4OUT & ~0x0F) | (~counter & 0x0F);
	}
#endif

	int pttype = 0;
	switch (wave) {
	case qrs_wave: pttype = 0x01; break;
	case p_wave:   pttype = 0x02; break;
	case t_wave:   pttype = 0x03; break;
	default:       pttype = 0x00; break;
	}

	switch (type) {
	case type_onset:    pttype |= 0x10; break;
	case type_peak:     pttype |= 0x20; break;
	case type_end:      pttype |= 0x30; break;
	case type_sec_peak: pttype |= 0x40; break;
	default:            pttype |= 0x00; break;
	}

	dint();
	putchar_uart1(0xED); /* Print point (5 bytes) */
	putchar_uart1(pttype); /* Point type (YTBDHTDI) */
	putchar_uart1((offset >> 24) & 0xff);
	putchar_uart1((offset >> 16) & 0xff);
	putchar_uart1((offset >>  8) & 0xff);
	putchar_uart1((offset >>  0) & 0xff);
	eint();

	resumetiming();
}
#endif

void wait(long long c) { 
	volatile long long i;        /* declare i as volatile int */
	for(i = 0; i < c; i++)
		;           
}

/* Dump the content of the stack */
static void dumpstack() {
	int* stack;
	int* pc;

	asm("mov r1, %[stack]\n\t" // Move the stack pointer to stack
	    "mov r0, %[pc]\n\t" // Move pc to stack
	    : [stack] "=r"(stack), [pc] "=r"(pc));

	printf("Stack is at %p\r\n", stack);

	printf("%04x", (int)pc);
	while ((int)stack <= 0x3900) {
		printf(":%04x", *stack);
		stack++;
	}
	printf("\r\n");
}

/* "Fixed" UART */
void init_uart0() {
	P3SEL = 0x30;           // P3.4 und P3.5 as USART0 TXD/RXD
	UCTL0 = CHAR | SWRST;   // 8 data bits, 1 stop bit, no parity
	ME1 |= UTXE0 | URXE0;   // Enable TX and RX
	UTCTL0 |= SSEL1;        // Use SMLCK as input (~8 MHz)
	/* Generator: http://mspgcc.sourceforge.net/baudrate.html */
	UBR00=0xD0; UBR10=0x00; UMCTL0=0x11; /* uart0 8000000Hz 38406bps */
	UCTL0 &= ~SWRST;        // start USART
}

/* Bluetooth UART */
void init_uart1() {
#if !SERIAL_DATA && !STATIC_DATA
	/* Bluetooth */

	/* Initialize the serial port anyway */
	UCTL1 = CHAR | SWRST;   // 8 data bits, 1 stop bit, no parity
	ME2 |= UTXE1 | URXE1;   // Enable TX and RX
	UTCTL1 |= SSEL1;        // Use SMLCK as input (~1 MHz)
	/* Generator: http://mspgcc.sourceforge.net/baudrate.html */
	UBR01=0x45; UBR11=0x00; UMCTL1=0xAA; /* uart1 8000000Hz 115107bps */
	UCTL1 &= ~SWRST;        // start USART

	/* Check if we are back from a soft reset, in that case no init is needed... nope... */
#if 0
	if (IFG1 & WDTIFG) {
		IFG1 &= ~WDTIFG; /* Clear WDTIFG */
		printf("Back from soft reset, no BT init needed\r\n");
		return;
	}
#endif
/*
TOSH_ASSIGN_PIN(BT_PIO,          1, 5);
TOSH_ASSIGN_PIN(BT_RTS,          1, 6);
TOSH_ASSIGN_PIN(BT_CTS,          1, 7);
TOSH_ASSIGN_PIN(BT_TXD,          3, 6);
TOSH_ASSIGN_PIN(BT_RXD,          3, 7);
TOSH_ASSIGN_PIN(BT_RESET,        5, 5);
*/
	// BT PINS
	/* Reset: 5.5, GPIO */
#define BT_RESET (0x01 << 5)
	P5DIR |=  BT_RESET; /* 5.5 */
	P5SEL &= ~BT_RESET;
	P5OUT &= ~BT_RESET; /* This switches off the radio (reset active-low) */

#define BT_RTS (0x01 << 6)
#define BT_CTS (0x01 << 7)
	/* RTS/CTS = 1.6/7, GPIO*/
	P1DIR &= ~BT_RTS;
	P1DIR |=  BT_CTS;
	P1SEL &= ~(BT_RTS | BT_CTS);

#define BT_PIO (0x01 << 5)
	P1DIR &= ~BT_PIO;
	P1SEL &= ~BT_PIO;

#define BT_TXD (0x01 << 6)
#define BT_RXD (0x01 << 7)
	/* TXD/RXD = 3.6/7, UART1 */
	P3DIR |=  BT_TXD;
	P3DIR &= ~BT_RXD;
	P3SEL |= (BT_TXD | BT_RXD);

	wait(10000LL);

	P5OUT |= BT_RESET; /* This activates the radio */

	P4OUT |= 0x08;

	printf("BTReset\r\n");

	wait(100000LL);

	P4OUT &= ~0x08;

	P1OUT &= ~BT_CTS; /* toggling cts wakes it up */
	wait(100LL);
	P1OUT |=  BT_CTS;
	wait(10000LL);
	P1OUT &= ~BT_CTS; /* toggling cts wakes it up */

	printf("BTToggle done\r\n");

	wait(100000LL);
#endif
}

void start_timer() {
#if SERIAL_DATA || STATIC_DATA
	/* Timer A control */
	/* Source from SMCLK, divide by 8, Continuous mode, enable the interrupt and clear status */
	TACTL = TASSEL_2 | ID_3 | MC_2 | TACLR | TAIE;
#else /* ADC data */
	/* Timer A control */
	/* Ensure the timer is stopped. */
	TACTL = 0;
	_BIC_SR(OSCOFF);

	/* Run the timer of the ACLK. */
	TACTL = TASSEL_1;
	/* Clear everything to start with. */
	TACTL |= TACLR;
	/* Set the compare match value according to the tick rate we want. */
	TACCR0 = 131; /* 32768/250 */
	/* Enable the interrupts. */
	TACCTL0 = CCIE;
	/* Start up clean. */
	TACTL |= TACLR;
	/* Up mode. */
	TACTL |= MC_1;
#endif
}

void init_adc() {
#if !SERIAL_DATA && !STATIC_DATA
	P6SEL = 0xff;

	ADC12CTL0 = MSC | REF2_5V | REFON | ADC12ON; /* Switch the ADC on */

	ADC12CTL1 = ADC12SSEL_1 | ADC12DIV_0 | CONSEQ_0 | SHS_0 | SHP; // | ADC12SSEL_1;

	ADC12MCTL0 = INCH_1 | SREF_1;

	ADC12CTL0 |= ENC | ADC12SC; /* Start conversion */
#endif
}

int main() {
	/* main function, called by startup-code */
	P4DIR = 0x0F;          /* port 4 = output           */
	P4OUT = 0x00;          /* Light all leds */

	/* Clock control */
	/* MCLK = 8Mhz */
	DCOCTL |= DCO2 | DCO1 | DCO0;
	BCSCTL1 |= RSEL2 | RSEL1 | RSEL0;

	BCSCTL1 &= ~(XT2OFF); /* Enable XT2, 8Mhz external oscillator */

	/* MCLK = DCOCLK, SMCLK = DCOCLK/8 */
	//BCSCTL2 = DIVS_3;
	/* MCLK = XT2 (8Mhz oscillator), SMCLK = XT2 (8Mhz oscillator) */
	BCSCTL2 = SELM_2 | SELS | DIVS_0;

	init_uart0();
	eint();
	init_uart1();

	init_adc();
	start_timer();

	putchar(RESET);
	P4OUT = 0x09;          /* Light 2 leds */

	PROF_START;

#if SERIAL_DATA
	readuart(); /* Read first block */
#endif
	putchar(START);
	printf("fill_in\r\n");
	putchar(END);
	stoptiming();
	uint64_t starttime = (localtime << 16 | TAR);
	resumetiming();
	fill_in();

	stoptiming();
	putchar(START);
	printf("li2\r\n");
	putchar(END);
	resumetiming();

	li2();
	stoptiming();

	uint64_t totaltime = (localtime << 16 | TAR)-starttime;

	P4OUT = 0x00;
	putchar(START);
	printf("total time=%lld ms\r\n", totaltime/1000);
	putchar(END);
	putchar(DONE);
	PROF_STOP;
	stopme();
	return 0;
}
#endif
